equations of motion with matlab code